/*
 * Javlov - a Java toolkit for reinforcement learning with multi-agent support.
 * 
 * Copyright (c) 2009 Matthijs Snel
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
package net.javlov.world;

import java.awt.geom.AffineTransform;
import java.awt.geom.Point2D;

import net.javlov.AbstractContinuousAction;
import net.javlov.Agent;
import net.javlov.ContinuousAction;

public class DifferentialWheelsAction extends AbstractContinuousAction {

	private static final long serialVersionUID = -306366193092042887L;

	private String name;
	protected transient World.Continuous world;
	
	//values for Khepera, measured in cm
	private double wheelRadius, //= 0.8,
					axleLength, //= 5.3,
					maxSpeed; //= 125 / (2*Math.PI); //125 revolutions per sec -> 1 m/s*/
	
	protected DifferentialWheelsAction() {
		setDimensionality(2);
		setValues(new double[2]);
		setName(getClass().toString());
	}
	
	public DifferentialWheelsAction(World.Continuous world, double wheelRadius, double axleLength, double maxSpeed ) {
		this();
		setWorld(world);
		setWheelRadius(wheelRadius);
		setAxleLength(axleLength);
		setMaxSpeed(maxSpeed);
	}
	
	public double getWheelRadius() {
		return wheelRadius;
	}

	public void setWheelRadius(double wheelRadius) {
		this.wheelRadius = wheelRadius;
	}

	public double getAxleLength() {
		return axleLength;
	}

	public void setAxleLength(double axleLength) {
		this.axleLength = axleLength;
	}

	public double getMaxSpeed() {
		return maxSpeed;
	}

	public void setMaxSpeed(double maxSpeed) {
		this.maxSpeed = maxSpeed;
	}

	public String getName() {
		return name;
	}
	
	public World getWorld() {
		return world;
	}
	
	protected void setName(String name) {
		this.name = name;
	}
	
	public void setWorld(World.Continuous world) {
		this.world = world;
	}
	
	//// ACTION METHOD ////
	@Override
	public void execute(Agent a) {
		Body abody = world.getAgentBody(a);

		//wheel speeds
		double leftv = values[0], rightv = values[1];
		if ( leftv == 0 && leftv == rightv )
			return;
		//speed of outer vs inner wheel on circle robot will describe
		double outerv, innerv;
		//direction of rotation, 1=clockwise
		int direction = 1;
		if ( leftv > rightv ) {
			outerv = leftv;
			innerv = rightv;
		} else {
			direction = -1;
			outerv = rightv;
			innerv = leftv;
		}
		//radius of circle that centre of robot will describe. Will be infinity if it's a 
		//straight line
		double r = ((outerv + innerv) * 0.5*axleLength) / (outerv - innerv);
		//distance it will travel along outer radius (fastest moving wheel)
		//double d = wheelRadius * outerv * maxSpeed * world.getTimeStep();
		double d = wheelRadius * outerv * maxSpeed * world.getTimeStep();
		//absolute angle of rotation it will go through
		double rot = (r == Double.POSITIVE_INFINITY ? 0 : d / (r + 0.5*axleLength));
		
		//now calc new location
		//angle between radius and line from curr to new loc
		double beta = (Math.PI - rot) / 2,
				//Euclidean distance between current loc and new loc
				l = (r == Double.POSITIVE_INFINITY ? d : 2*r*Math.cos(beta)),
				//new bearing
				newrot = abody.getBearing() + direction * (0.5*Math.PI - beta);
		//construct a vector of length l, rotate it and add it to current position vector.
		//this gives new location
		Point2D.Double vecToNewLoc = new Point2D.Double(l, 0);
		AffineTransform.getRotateInstance(newrot).deltaTransform(vecToNewLoc, vecToNewLoc);

		//execute the action
		world.translateBody(abody, vecToNewLoc.x, vecToNewLoc.y);
		world.rotateBody(abody, rot*direction);
	}
	
	
	//// CONTINUOUS ACTION METHODS ////
	@Override
	public double getMaxRange() {
		return 1;
	}

	@Override
	public double getMinRange() {
		return 0;
	}
}
